// Copyright (c) 2010, University of Southern Denmark
// All rights reserved.
// 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
// 3. Neither the name of the University of Southern Denmark nor the names of
//    its contributors may be used to endorse or promote products derived from
//    this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE UNIVERSITY OF SOUTHERN DENMARK BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#ifndef RANSACPROBLEM_H
#define RANSACPROBLEM_H

#include <iostream>

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <sys/time.h>
#include <limits>
#include <vector>

using namespace std;

// #define RANSAC_PRINTS

/*! Parent RANSAC Problem class */
template <class T, class V> class RANSACproblem 
{
 public:         
  /*! \brief Construct RANSAC problem
    \param nInputBuffers number of inputbuffers the intermediate uses for input
  */
  RANSACproblem(unsigned int minIter, unsigned int maxIter, double sigma, unsigned int minSamples)
    {
      // std::cout << "RANSACproblem Constructor is called" << std::endl;
                        
      _minIter = minIter;
      _maxIter = maxIter;
      _sigma = sigma;
      _minSamples = minSamples;
                        
      _doReestimation = true;
                        
      //we need to allocate memory
      estimate = new V(4);
      bestEstimate = new V(4);
      //bestConsensusSet_Compare = NULL;
    }

  ~RANSACproblem()
    {
                        
    }
                
  void fitModel() {
    //initialize randomizer
    srand ( time(NULL) );
        
    //BEGIN: main loop
        
    //size of the input set
    //                         unsigned int inputSetSize = inputSet->size();

    bestRankingScore = inputSet->size()*100000;
        
    //number of iterations
    unsigned int iterCnt = 0, fullIterCnt = 0;
        
    //threshold on the number of iterations
    unsigned int iterThreshold = _maxIter;
        
    //number of iterations with no updates
    unsigned int noUpdateCnt = 0;
        
    bool /*updateBestSet, */updateIterThreshold;
        
    //squared error - should ideally be calculated based on sigma
    //                      double noiseSquared = 2.0; //6.6363;
        
    //some more initializations
    unsigned int inlierCnt, inlierCntBest = 0; // k
    double rankingScore;
    double q;
    double eps = std::numeric_limits<double>::epsilon();
						
#ifdef RANSAC_PRINTS
    timeval timeOfDay;
    double startTime, endTime, totalStartTime, totalEndTime;
#endif
        
    while(iterCnt <= _minIter || ((iterCnt <= iterThreshold) && (iterCnt <= _maxIter))) { // && (no_updates <= _maxNoUpdates)

#ifdef RANSAC_PRINTS
      gettimeofday(&timeOfDay, NULL);
      startTime = timeOfDay.tv_sec*1000000.0+(timeOfDay.tv_usec);
      totalStartTime = startTime;
#endif

      //update the number of iterations
      iterCnt++;
                
      //                              updateBestSet = false;
      updateIterThreshold = false;

      //produce a new sample-set from the input set
      makeSampleSet();
								
#ifdef RANSAC_PRINTS
      gettimeofday(&timeOfDay, NULL);
      endTime = timeOfDay.tv_sec*1000000.0+(timeOfDay.tv_usec);
								
      std::cout << "makeSampleSet() takes time = " << endTime - startTime << std::endl;
								
      startTime = endTime;
#endif
                                
      //fit a model to the sample set
      if (!fitModelToSampleSet()) continue;
								
#ifdef RANSAC_PRINTS
      gettimeofday(&timeOfDay, NULL);
      endTime = timeOfDay.tv_sec*1000000.0+(timeOfDay.tv_usec);
								
      std::cout << "fitModelToSampleSet() takes time = " << endTime - startTime << std::endl;
								
      startTime = endTime;
#endif

      //update the number of full iterations
      fullIterCnt++;
                                
      //from the fitted model find the consensus-set
      //rankingScore = findConsensusSet();
      rankingScore = getFitScore();
								
#ifdef RANSAC_PRINTS
      gettimeofday(&timeOfDay, NULL);
      endTime = timeOfDay.tv_sec*1000000.0+(timeOfDay.tv_usec);
								
      std::cout << "findConsensusSet() takes time = " << endTime - startTime << std::endl;
								
      startTime = endTime;
#endif
                                
      //                              //get the estimated number of inliers - here the different RANSAC implementations should come in to play
      inlierCnt = inputSet->size()-rankingScore;

#ifdef RANSAC_PRINTS
      printf("Nr of points in input: %d, rankScore: %d, nrOfInliers: %d\n", inputSet->size(), rankingScore, inlierCnt);
#endif
                
      //Update --------------------------------------------------------------

      //if we found a better(smaller) rankingScore set update both the inlier set and the
      //number of iterations
      if(rankingScore < bestRankingScore) {
	updateIterThreshold = true;
	inlierCntBest = inlierCnt;
                        
	updateBestEstimate();
	bestRankingScore = rankingScore;
                        
	noUpdateCnt = 0;
                        
	//                                      updateBestSet = true;
      }
      else {
	noUpdateCnt++;
      }

      //update the number of iterations
      if(updateIterThreshold) {

	q = get_q_RANSAC(inputSet->size(), inlierCntBest);
                        
	//                                      std::cout << "q=" << q << std::endl;
	//                                      std::cout << "eps=" << eps << std::endl;
                        
	if (q > eps) {
	  iterThreshold = get_iter_RANSAC(eps, q);
	}
      }

#ifdef RANSAC_PRINTS
      if(updateIterThreshold /*|| updateBestSet*/) {
	printf("Iteration = %5d/%9d.\n", iterCnt, iterThreshold);
	//                                         printf("FullIteration = %5d/%9d.\n", fullIterCnt, iterThreshold);
	printf("Ranking = %6f\n", bestRankingScore);
	printf("Inliers = %6d/%6d\n", inlierCntBest, inputSet->size());
      }
#endif

      //if all the points have been assigned then exit the loop
      if (inputSet->size() == inlierCnt && rankingScore == bestRankingScore) {
#ifdef RANSAC_PRINTS
	std::cout << "all points are in consensusSet" << std::endl;
#endif
	break;
      }
								
#ifdef RANSAC_PRINTS
      gettimeofday(&timeOfDay, NULL);
      totalEndTime = timeOfDay.tv_sec*1000000.0+(timeOfDay.tv_usec);
								
      std::cout << "whileLoop takes time = " << totalEndTime - totalStartTime << std::endl;
#endif
    }
						
#ifdef RANSAC_PRINTS
    printf("Iteration = %5d/%9d.\n", iterCnt, iterThreshold);
    printf("Inliers without reestimation = %6d/%6d\n", inlierCntBest, inputSet->size());			
#endif
						
    //to obey the requirement of minimum 6 rbm constraints we need to have more than 3 contours in consensus
    if(_doReestimation && inlierCntBest > 2) {
      //Reestimation --------------------------------------------------------
#ifdef RANSAC_PRINTS								
      std::cout << "Restimating the parameter vector... " << std::endl;
#endif
      performReestimation();          
#ifdef RANSAC_PRINTS
      printf("Inliers after reestimation = %6d/%6d\n", consensusSet.size(), inputSet->size());
#endif
    }
  };
        
  unsigned int getMinimumIterations( ) {
    return _minIter;
  }
                
  unsigned int getMaximumIterations( ) {
    return _maxIter;
  }
                
  unsigned int getSigma( ) {
    return _sigma;
  }
                
  bool getDoReestimation() {
    return _doReestimation;
  }
                
  /*! \brief Set whether reestimation shall be performed
    \param doReestimation true or false
  */
  void setDoReestimation(bool doReestimation) {
    _doReestimation = doReestimation;
  }

  virtual void setInputSet(std::vector<T *> &iSet) {
    inputSet = &iSet;
  }

  std::vector<T *> * ConsensusSet() {
    return &consensusSet;
  }
  /*
  std::vector<T *> * getBestConsensusSet() {
    return bestConsensusSet;
  }

  std::vector<T *> * getBestOutliersSet() {
    return bestOutliersSet;
  }
  */

  V * getBestEstimate(){
    return bestEstimate;
  }
                
 private:
  //make the random sampleset
  virtual void makeSampleSet() {
    unsigned int j;
    unsigned int tempVal;
    unsigned int counter = 0;
    std::vector< unsigned int > randNums(_minSamples);
    unsigned int inputSetSize = inputSet->size();
                        
    sampleSet.clear();
                        
    while(counter < _minSamples) {                          
      tempVal = rand() % inputSetSize;
                                
      for (j=0;j<counter;j++) {
	if (randNums[j] == tempVal)
	  break;
      }

      if (j==counter) {
	randNums[counter] = tempVal;
	sampleSet.push_back(inputSet->at(tempVal));
	counter++;
      }
    }
                        
    //                      std::cout << "SampleSet: (" << sampleSet.at(0)->x << "," << sampleSet.at(0)->y << ") - (" << sampleSet.at(1)->x << "," << sampleSet.at(1)->y << ")" << std::endl;
  };
                                
  virtual bool fitModelToData(std::vector<T *> &data) = 0;
                
  virtual bool fitModelToSampleSet() {
    return fitModelToData(sampleSet);
  };
                
  /*! \brief finds Consensus set and returns a ranking score for the given parameters estimate
    \return   double - the ranking of the given solution

    The simplest way to rank an estimate is to count the number of outliers. The smallest
    number - the better rank:
    return outliersSet->size();   or if not counting outliers then
    return inputSet->size() - consensusSet->size();

    But instead of just taking number of outliers as a rankingScore, we can make it
    a bit more sophisticated with no additional cost (RANSAC for dummies, page 21.)
    if we calculate the ranking score as:

    ranking score = SUM (ρ)              , where

    (
    (     error,    error ≤ δ
    ρ = (
    (       δ,      otherwise
    (

    This is calculated at almost no additional cost as error is calculated for each
    point anyway and expression (error ≤ δ) is evaluated anyway.
  */
  virtual double getFitScore() = 0;
  virtual void getBestConsensusSet() = 0;
  //virtual double findConsensusSet() = 0;
                
  void updateBestEstimate() {
    //if outliers set is needed, then it should be
    //populated inside "findConsensusSet" method

    *((V *)bestEstimate) = *((V *)estimate);
						
						
						
    //if(bestConsensusSet_Compare != NULL)
    //  delete bestConsensusSet_Compare;
						
    //bestConsensusSet_Compare = consensusSet_Compare;
    //consensusSet_Compare = new std::vector<T *>;
  };
                
  void performReestimation() {
    getBestConsensusSet();
    fitModelToData(consensusSet);
    bestRankingScore = getFitScore();
    updateBestEstimate();
  };
                
  virtual void printBestEstimate() = 0;
                
  double get_q_RANSAC(int N, int N_I) {
    double q;
        
    std::vector<double> den(_minSamples), num(_minSamples);
        
    for(unsigned int i=0;i<_minSamples;i++) {
      num[i]=(double)N_I-i;
      den[i]=(double)N-i;
    }
        
    q = num[0] / den[0];
        
    for(unsigned int i=1;i<_minSamples;i++) {
      q *= num[i] / den[i];
    }
        
    return q;
  };
                
  unsigned int get_iter_RANSAC(double eps, double q) {
    if((1.0 - q) > 1e-12)
      return (int) ceil( log(eps) / log(1.0 - q) );
    else
      return 0;
  };

 protected:
  unsigned int _minIter, _maxIter, _minSamples;
  double _sigma;
  double bestRankingScore;
  //unsigned int inputSetSize_Primitives;
  //unsigned int consensusSetSize_Primitives;
                
  std::vector<T *> * inputSet;
  std::vector<T *> sampleSet;
  V *estimate;
  V *bestEstimate;
				
  std::vector<T *> consensusSet;

  //std::vector<T *> * consensusSet_Compare;
  //std::vector<T *> * bestConsensusSet_Compare;
                
  bool _doReestimation;
};

#endif
